#include "MeasurE616AlgBackRcEdge.h"

MeasurE616AlgBackRCEdge::MeasurE616AlgBackRCEdge(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgBackRCEdge::operator()(cv::Mat &srcImg,
                                         int id,
                                         std::map<string, float> &measRes,
                                         int fixtureId)
{
//矩形边缘提取   //RC back图 \paras616\back\RC.xml ；
//int NewGetRectEdge(cv::Mat srcimg, const vector<int> paras, vector<float>& dists, vector<vector<cv::Point2f>>& contourres);

        std::vector<std::vector<cv::Point2f>> contourres;

        std::vector<int> tparas;
        int i=8,tg=0;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(0,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();

        std::vector<float> dists;
        vector<vector<cv::Point2f>> contourres0;
        MeasurE616EdgeDetect obj;
        obj.NewGetRectEdge(imgt, tparas,dists,contourres0);
        for(auto &ci:contourres0)
        {
            ImageCoordinateTransformation(ci,rangVec);
        }
        contourres.resize(contourres0.size());
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }

        for(auto &ci:contourres0)
        {

           for(auto &cj:ci)
           {
               cv::Point2f ptsOut;
              CoordinateTransformationRelateToDot(datumBackVec[id],cj,ptsOut);
              contourres[tg].push_back(ptsOut);
           }
           tg++;
        }

        std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";

        cout<<contourres.size()<<endl;
        float fai20x1=0;
        for(int i=0;i<contourres[2].size();++i){
            fai20x1+=contourres[2][i].x/contourres[2].size();
        }
        float fai20x2=0;
        for(int i=0;i<contourres[4].size();++i){
            fai20x2+=contourres[4][i].x/contourres[4].size();
        }
        measRes.insert(std::make_pair(string("FAI20-RC-X1-distance_pixel"), fabs(fai20x1-fai20x2)));
        float fai20_1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI20-RC-X1-distance")], fabs(fai20x1-fai20x2));
        measRes.insert(std::make_pair(string("FAI20-RC-X1-distance_real"), fai20_1));

        float fai20x3=0;
        for(int i=0;i<contourres[3].size();++i){
            fai20x3+=contourres[3][i].x/contourres[3].size();
        }
        float fai20x4=0;
        for(int i=0;i<contourres[5].size();++i){
            fai20x4+=contourres[5][i].x/contourres[5].size();
        }
        measRes.insert(std::make_pair(string("FAI20-RC-X2-distance_pixel"), fabs(fai20x3-fai20x4)));
        float fai20_2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI20-RC-X2-distance")], fabs(fai20x3-fai20x4));
        measRes.insert(std::make_pair(string("FAI20-RC-X2-distance_real"), fai20_2));

        double distsum=0,distsum1=0;
        for(int i=0;i<contourres[0].size();++i){
            distsum+=contourres[0][i].y;
        }
        distsum/=contourres[0].size();
        for(int i=0;i<contourres[1].size();++i){
            distsum1+=contourres[1][i].y;
        }
        distsum1/=contourres[1].size();
        float fai68=fabs(distsum1-distsum);
        measRes.insert(std::make_pair(string("FAI68-RC-Y-distance_pixel"),fai68));
        float fai68rcy = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI68-RC-Y-distance")], fai68);
        measRes.insert(std::make_pair(string("FAI68-RC-Y-distance_real"), fai68rcy));

        vector<cv::Point2f> leftcontour=contourres[2];
        leftcontour.insert(leftcontour.end(),contourres[3].begin(),contourres[3].end());
        vector<cv::Point2f> rightcontour=contourres[4];
        rightcontour.insert(rightcontour.end(),contourres[5].begin(),contourres[5].end());
//        MeasurE616EdgeDetect obj;
        float fai69=obj.GetLine2LineMid(leftcontour,rightcontour,1);

        measRes.insert(std::make_pair(string("FAI69-RC-X-distance_pixel"),-fai69));
        float fai69rcx = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI69-RC-X-distance")], -fai69);
        measRes.insert(std::make_pair(string("FAI69-RC-X-distance_real"), fai69rcx));

        float fai69x = m_standardValMap["FAI69-RC-X-distance"];
        float fai69PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI69-RC-X-distance")],
                fai69x);
        double fai69xPosPixel = 2*sqrt((-fai69-fai69PosPixelX)*(-fai69-fai69PosPixelX));
        measRes.insert(std::make_pair(string("FAI69-RC-X-position_pixel"), fai69xPosPixel));
        float fai69XPosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI69-RC-X-position")], fai69xPosPixel);
        measRes.insert(std::make_pair(string("FAI69-RC-X-position_real"), fai69XPosDis));


        float fai70=obj.GetLine2LineMid(contourres[0],contourres[1],2);
        measRes.insert(std::make_pair(string("FAI70-RC-Y-distance_pixel"),-fai70));
        float fai70rcy = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI70-RC-Y-distance")], -fai70);
        measRes.insert(std::make_pair(string("FAI70-RC-Y-distance_real"), fai70rcy));

        float fai70y = m_standardValMap["FAI70-RC-Y-distance"];
        float fai70PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI70-RC-Y-distance")],
                fai70y);
        double fai70yPosPixel = 2*sqrt((-fai70-fai70PosPixelY)*(-fai70-fai70PosPixelY));
        measRes.insert(std::make_pair(string("FAI70-RC-Y-position_pixel"), fai70yPosPixel));
        float fai70yPosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI70-RC-Y-position")], fai70yPosPixel);
        measRes.insert(std::make_pair(string("FAI70-RC-Y-position_real"), fai70yPosDis));
}
